/*
 * Camera.c
 *
 *  Created on: Nov 7, 2013
 *      Author: Efrain
 */

#include "derivative.h"
#include "Mytypes.h"
#include "ADC.h"
#include "PIT.h"
#include "Camera.h"
#include "ServoMotor.h"

uint8 ENABLE_SI;
uint8 u8_Concatenated_Frame[200];
uint8 u8_left_index = 0;
uint8 u8_right_index = 0;
uint8 u8_Line_Center = 0;
/**************************************************************
 *  Name                 : vdfn_Camera_Init
 *  Description          : Initialises Camera
 *  Parameters           : none
 *  Return               : none
 *  Critical/explanation : no
 **************************************************************/
void vdfn_Camera_Init(void){
	SIM_SCGC4 |=  SIM_SCGC4_SPI1_MASK; 		//Enables Clock Gating for SPI Module
	SPI1_BR = SPI_BR_SPPR(7); 
	SPI1_C1 = SPI_C1_SPE_MASK | SPI_C1_MSTR_MASK | SPI_C1_CPHA_MASK; //SPE = 1, CPHA = 1 (Default, RM is wrong, Default = 0), MSTR = 1, CPOL = 0 (Default)
	
	(void)SPI1_S;
	SPI1_D = 0x55;
}

/**************************************************************
 *  Name                 : vdfn_Send_Enable_Camera
 *  Description          : Sends an Enable in order to start both cameras acquiring.
 *  Parameters           : none
 *  Return               : none
 *  Critical/explanation : no
 **************************************************************/
void vdfn_Send_Enable_Camera(void){
	ENABLE_SI = 1;
}


/**************************************************************
 *  Name                 : vdfn_Get_Camera_Frame
 *  Description          : Concatenates two cameras frames, truncates n-pixels and normalises vector. (cero or one)
 *  Parameters           : none
 *  Return               : none
 *  Critical/explanation : no
 **************************************************************/
void vdfn_Get_Camera_Frame(void){
	uint8 u8_pixel_index = 0;
	/* Starts Concatenation and truncation*/
	for(;u8_pixel_index <=100;u8_pixel_index++){
		u8_Concatenated_Frame[u8_pixel_index] = u8_ADC_Camera_1[u8_pixel_index + TRUNCATED_PIXELS];
		u8_Concatenated_Frame[200-u8_pixel_index] = u8_ADC_Camera_2[100 + TRUNCATED_PIXELS - u8_pixel_index];
	}
	
	/* Getting an array of just 1's and 0's */
	for(u8_pixel_index = 0; u8_pixel_index <= 200; u8_pixel_index++){
		if(u8_Concatenated_Frame[u8_pixel_index] > 180) 			//Threshold -> 180
			u8_Concatenated_Frame[u8_pixel_index] = 1;
		else 
			u8_Concatenated_Frame[u8_pixel_index] = 0;
	}

	
	/* Start the line positioning algorithm */
	vdfn_Get_Line_Center();
}

/**************************************************************
 *  Name                 : vdfn_Get_Line_Center
 *  Description          : Analyses concatenated vector and obtains the line center
 *  Parameters           : none
 *  Return               : none
 *  Critical/explanation : no
 **************************************************************/

void vdfn_Get_Line_Center(void){
	uint8 u8_pixel_index = 0;
	uint8 Safety_Value = 0;
	uint8 LEFT_FLAG = 0;
	uint8 RIGHT_FLAG = 0;
	
	/* Left approach */
	for(; u8_pixel_index < 200 ; u8_pixel_index++){
			if(u8_Concatenated_Frame[u8_pixel_index] == 0){
				if((u8_Concatenated_Frame[u8_pixel_index+1]) && (u8_Concatenated_Frame[u8_pixel_index+2]) && (u8_Concatenated_Frame[u8_pixel_index+3]) /
						(u8_Concatenated_Frame[u8_pixel_index+4]) && (u8_Concatenated_Frame[u8_pixel_index+5])){
					u8_left_index = u8_pixel_index; //Save the first black pixel position
					u8_pixel_index = 201;			//You've got your line, then finish the for cycle 
					LEFT_FLAG = 1;
				}
			}
	}
	
	/*
		Si no encontro la linea se queda con el valor de index del frame pasado (funciona tanto para left como para right approaching)
	*/
	
	
	/* Right approach */
	
	for(u8_pixel_index = 199; u8_pixel_index > 1; u8_pixel_index--){
				if(u8_Concatenated_Frame[u8_pixel_index] == 0){
					if((u8_Concatenated_Frame[u8_pixel_index-1]) && (u8_Concatenated_Frame[u8_pixel_index-2]) && (u8_Concatenated_Frame[u8_pixel_index-3]) /
							(u8_Concatenated_Frame[u8_pixel_index-4]) && (u8_Concatenated_Frame[u8_pixel_index-5])){
						u8_right_index = u8_pixel_index; //Save the first black pixel position
						u8_pixel_index = 1;			//You've got your line, then finish the for cycle
						RIGHT_FLAG = 1;
					}
				}
		}
	
	Safety_Value = (u8_right_index + u8_left_index)/2;
	
	if(RIGHT_FLAG == 1 && LEFT_FLAG == 1){
		
		if((Safety_Value - u8_Line_Center) < 45 ){u8_Line_Center = (u8_left_index + u8_right_index) / 2;}
		
			else { u8_Line_Center = u8_Line_Center;}
		
		
		
	}
	
}


/**************************************************************
 *  Name                 : vdfn_Camera_Calibration
 *  Description          : Calibrates both cameras
 *  Parameters           : none
 *  Return               : none
 *  Critical/explanation : no
 **************************************************************/

void vdfn_Camera_Calibration(void){
	u8_ADC_Camera_2;
	u8_ADC_Camera_1;
}
